/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_Studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#include "图形相交.h"
#include "matXX.h"


bool IntersectTriangle(const float* vert1, const float* vert2, const float* vert3, const float* near, const float* far, float* uvt){
	float edge1[3] = {0.0, 0.0, 0.0};
	float edge2[3] = {0.0, 0.0, 0.0};

	//变向量计算
	edge1[0] = vert2[0] - vert1[0];
	edge1[1] = vert2[1] - vert1[1];
	edge1[2] = vert2[2] - vert1[2];

	edge2[0] = vert3[0] - vert1[0];
	edge2[1] = vert3[1] - vert1[1];
	edge2[2] = vert3[2] - vert1[2];


	//计算射线方向
	float dir[3] = {far[0]-near[0], far[1]-near[1], far[2]-near[2]};
	//double dirW = sqrt( pow(dir[0], 2.0) + pow(dir[1], 2.0) + pow(dir[1], 2.0));
	double dirW = sqrt(fabs(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]));
	dir[0] /= dirW;
	dir[1] /= dirW;
	dir[2] /= dirW;

	//叉积计算
	float pvec[3];
	//-------------------不要归一化--------------------
	pvec[0]= dir[1]*edge2[2] - dir[2]*edge2[1];
	pvec[1]= dir[2]*edge2[0] - dir[0]*edge2[2];
	pvec[2]= dir[0]*edge2[1] - dir[1]*edge2[0];

	float det = edge1[0] * pvec[0] + edge1[1] * pvec[1] + edge1[2] * pvec[2];
	float tvec[3];
	//判断是否正面穿过
	if ( det > 0 )  {
		//-------------------不要归一化--------------------
		tvec[0] = near[0] - vert1[0];
		tvec[1] = near[1] - vert1[1];
		tvec[2] = near[2] - vert1[2];
		//qDebug()<<"正面";
	}
	else  {
		//-------------------不要归一化--------------------
		tvec[0] = vert1[0] - near[0];
		tvec[1] = vert1[1] - near[1];
		tvec[2] = vert1[2] - near[2];
		det = -det ;
		//qDebug()<<"反面";
	}

	if( det < 0.000000000001f )    return false;

	//u
	uvt[0] = tvec[0]*pvec[0]+ tvec[1]*pvec[1]+ tvec[2]*pvec[2];
	if( uvt[0] < 0.0f || uvt[0] > det ){
		//qDebug()<<"跳出1";
		return false;
	}
	float qvec[3] = {tvec[1]*edge1[2] - tvec[2]*edge1[1], tvec[2]*edge1[0] - tvec[0]*edge1[2], tvec[0]*edge1[1] - tvec[1]*edge1[0]};

	//v
	uvt[1] = dir[0]*qvec[0] + dir[1]*qvec[1] + dir[2]*qvec[2];
	if( uvt[1] < 0.0f || uvt[0] + uvt[1] > det ){
		//qDebug()<<"跳出2";
		return false;
	}
	//t
	uvt[2] = edge2[0]*qvec[0] + edge2[1]*qvec[1] + edge2[2]*qvec[2];
	float fInvDet = 1.0f / det;
	uvt[0] *= fInvDet;
	uvt[1] *= fInvDet;
	uvt[2] *= fInvDet;

	return true;
}

bool FrontIntersectTriangle(const float* vert1, const float* vert2, const float* vert3,
			    const float* near, const float* far, float* uvt, bool isFrant)
{
	float edge1[3] = {0.0, 0.0, 0.0};
	float edge2[3] = {0.0, 0.0, 0.0};

	//变向量计算
	edge1[0] = vert2[0] - vert1[0];
	edge1[1] = vert2[1] - vert1[1];
	edge1[2] = vert2[2] - vert1[2];

	edge2[0] = vert3[0] - vert1[0];
	edge2[1] = vert3[1] - vert1[1];
	edge2[2] = vert3[2] - vert1[2];


	//计算射线方向
	float dir[3] = {far[0]-near[0], far[1]-near[1], far[2]-near[2]};
	//double dirW = sqrt( pow(dir[0], 2.0) + pow(dir[1], 2.0) + pow(dir[1], 2.0));
	double dirW = sqrt(fabs(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]));
	dir[0] /= dirW;
	dir[1] /= dirW;
	dir[2] /= dirW;

	//叉积计算
	float pvec[3];
	pvec[0]= dir[1]*edge2[2] - dir[2]*edge2[1];
	pvec[1]= dir[2]*edge2[0] - dir[0]*edge2[2];
	pvec[2]= dir[0]*edge2[1] - dir[1]*edge2[0];

	float det = edge1[0] * pvec[0] + edge1[1] * pvec[1] + edge1[2] * pvec[2];
	float tvec[3];
	//判断是否正面穿过
	if ( det > 0 )  {
		if(!(isFrant)) return false;
		tvec[0] = near[0] - vert1[0];
		tvec[1] = near[1] - vert1[1];
		tvec[2] = near[2] - vert1[2];
		//qDebug()<<"正面";
	}
	else  {
		if(isFrant) return false;
		tvec[0] = vert1[0] - near[0];
		tvec[1] = vert1[1] - near[1];
		tvec[2] = vert1[2] - near[2];
		det = -det ;
		//qDebug()<<"反面";
	}

	if( det < 0.000000000001f )    return false;

	//u
	uvt[0] = tvec[0]*pvec[0]+ tvec[1]*pvec[1]+ tvec[2]*pvec[2];
	if( uvt[0] < 0.0f || uvt[0] > det ){
		//qDebug()<<"跳出1";
		return false;
	}
	float qvec[3] = {tvec[1]*edge1[2] - tvec[2]*edge1[1], tvec[2]*edge1[0] - tvec[0]*edge1[2], tvec[0]*edge1[1] - tvec[1]*edge1[0]};

	//v
	uvt[1] = dir[0]*qvec[0] + dir[1]*qvec[1] + dir[2]*qvec[2];
	if( uvt[1] < 0.0f || uvt[0] + uvt[1] > det ){
		//qDebug()<<"跳出2";
		return false;
	}
	//t
	uvt[2] = edge2[0]*qvec[0] + edge2[1]*qvec[1] + edge2[2]*qvec[2];
	float fInvDet = 1.0f / det;
	uvt[0] *= fInvDet;
	uvt[1] *= fInvDet;
	uvt[2] *= fInvDet;



	return true;
}

int32 f_graph_IntersectTriangle(const vec3& vert1, const vec3& vert2, const vec3& vert3, const vec3& S, const vec3& dir, vec3* uvt) {
	int32 相交情况 = 0;

	vec3 edge1 = { 0.0f, 0.0f, 0.0f };
	vec3 edge2 = { 0.0f, 0.0f, 0.0f };

	//边向量计算
	edge1 = vec_sub(vert2, vert1);
	edge2 = vec_sub(vert3, vert1);

	//叉积计算
	vec3 pvec = vec_cross(dir, edge2);

	float32 det = vec_dot(edge1, pvec);
	vec3 tvec;
	//判断是否正面穿过
	if (det > 0) {
		//-------------------不要归一化--------------------
		tvec = vec_sub(S, vert1);
		相交情况 = 1;
	}
	else {
		//-------------------不要归一化--------------------
		//反面
		tvec = vec_sub(vert1, S);
		det = -det;
		相交情况 = 2;	
	}
	if (det < 0.00000001f)    return 0;

	//vec_normalize(&tvec);
	//u
	uvt->x = vec_dot(tvec, pvec);
	if (uvt->x < 0.0f || uvt->x > det) return 0;
	
	vec3 qvec = vec_cross(tvec, edge1);

	//v
	uvt->y = vec_dot(dir, qvec);
	if (uvt->y  < 0.0f || uvt->x + uvt->y  > det) {
		return 0;
	}
	//t
	uvt->z = vec_dot(edge2, qvec);
	
	//float fInvDet = 1.0f / det;
	vec_mul(uvt, 1.0f / det);

	*uvt = dir * uvt->z + S;
	
	return 相交情况;
}

int32 f_graph_射线三角形相交(const vec3& vert, const vec3& 边A, const vec3& 边B, const vec3& S, const vec3& dir, vec3* uvt) {
	int32 相交情况 = 0;

	//叉积计算
	vec3 pvec = vec_cross(dir, 边B);

	float32 det = vec_dot(边A, pvec);
	vec3 tvec;
	//判断是否正面穿过
	if (det > 0) {
		//-------------------不要归一化--------------------
		tvec = vec_sub(S, vert);
		相交情况 = 1;
	}
	else {
		//-------------------不要归一化--------------------
		tvec = vec_sub(vert, S);
		det = -det;
		相交情况 = 2;
	}
	if (det < 0.00000001f)    return 0;

	//vec_normalize(&tvec);
	//u
	uvt->x = vec_dot(tvec, pvec);
	if (uvt->x < 0.0f || uvt->x > det) return 0;

	vec3 qvec = vec_cross(tvec, 边A);

	//v
	uvt->y = vec_dot(dir, qvec);
	if (uvt->y  < 0.0f || uvt->x + uvt->y  > det) {
		return 0;
	}
	//t
	uvt->z = vec_dot(边B, qvec);

	float fInvDet = 1.0f / det;
	vec_mul(uvt, fInvDet);

	*uvt = dir * uvt->z + S;

	return 相交情况;
}


bool PointinTriangle(const float* A, const float* B, const float* C, const float* P) {
	float v0[3] = {C[0] - A[0], C[1] - A[1], C[2] - A[2]};
	float v1[3] = {B[0] - A[0], B[1] - A[1], B[2] - A[2]};
	float v2[3] = {P[0] - A[0], P[1] - A[1], P[2] - A[2]};

	//float dot00 = v0.Dot(v0) ;
	float dot00 = vec_dot(v0, v0);
	float dot01 = vec_dot(v0, v1);
	float dot02 = vec_dot(v0, v2);
	float dot11 = vec_dot(v1, v1);
	float dot12 = vec_dot(v1, v2);

	float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ;
	float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ;
	if (u < 0 || u > 1) // if u out of range, return directly
	{
		return false ;
	}

	float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ;
	if (v < 0 || v > 1) // if v out of range, return directly
	{
		return false ;
	}

	return u + v <= 1 ;
}



















//, const vec3 两线段起点向量







int32 f_UpProject还原空间顶点(const vec3* xyz, const float32* 投影矩阵, const float32* 模型矩阵, const vec4* viewport, vec4* vert)
{
	float32 最终矩阵[16];
	float32 转置矩阵[16];

	f_Mat4X4_相乘(最终矩阵, 投影矩阵, 模型矩阵);

	if (f_Matrix4X4_InvertMatrixf2(最终矩阵, 转置矩阵) == 0) return 0;

	float32 in[4], out[4] = { 0.0,0.0,0.0,1.0 };

	//视口 -1 到 1 归一化
	in[0] = (xyz->x - viewport->x) / (viewport->z / 2) - 1;
	in[1] = (xyz->y - viewport->y) / (viewport->w / 2) - 1;
	in[2] = -xyz->z + 1;
	in[3] = 1.0;

	Matrix4X4_TranformVec4(out, 转置矩阵, in);
	if (out[3] == 0.0) return 0;

	out[3] = 1.0f / out[3];
	vert->x = out[0] * out[3];
	vert->y = out[1] * out[3];
	vert->z = out[2] * out[3];
	vert->w = 1.0;

	return 1;
}

vec3 f_UpProject还原空间顶点(const vec3& xyz, const Mat44& 投影矩阵, const Mat44& 视图矩阵, const S_Rect2Df& viewport) {
	auto mat = 投影矩阵 * 视图矩阵;

	Mat44 逆矩阵;
	f_Matrix4X4_InvertMatrixf2((float32*)mat.columns, (float32*)逆矩阵.columns);
	//mat = f_逆矩阵(投影矩阵);
	//逆矩阵 = f_求逆矩阵(mat);
	
	vec4 in = { 0,0,0,1 };
	vec3 out = { 0,0,0 };

	in.x = (xyz.x - viewport.offset.x) / (viewport.extent.x / 2) - 1;
	in.y = (xyz.y - viewport.offset.y) / (viewport.extent.y / 2) - 1;
	in.z = -xyz.z + 1;
	//in.z = xyz.z;
	in.w = 1.0;

	in = 逆矩阵 * in;
	
	if (in.w == 0) return out;
	
	in.w = 1.0f / in.w;

	out.x = in.x * in.w;
	out.y = in.y * in.w;
	out.z = in.z * in.w;

	return out;
}

vec3 f_UpProject还原空间顶点(const vec3& xyz, const Mat44f& 投影矩阵, const Mat44f& 视图矩阵, const S_Rect2Df& viewport) {
	auto mat = 投影矩阵 * 视图矩阵;
	//Mat44f 逆矩阵 = f_mat44_Inverse(mat);
	vec4 in = { 0,0,0,1 };
	vec3 out = { 0,0,0 };

	in.x = (xyz.x - viewport.offset.x) / (viewport.extent.x / 2) - 1;
	in.y = (xyz.y - viewport.offset.y) / (viewport.extent.y / 2) - 1;
	in.z = -xyz.z + 1;
	in.w = 1.0;

	in = mat * in;

	if (in.w == 0) return out;

	in.w = 1.0f / in.w;

	out.x = in.x * in.w;
	out.y = in.y * in.w;
	out.z = in.z * in.w;

	return out;
}

vec2 f_Project空间坐标转屏幕(const Mat44& 投影矩阵, const Mat44& 视图矩阵, const vec3& 中心坐标, const S_Rect2Df& rect) {
	vec4 in;
	in.x = 中心坐标.x;
	in.y = 中心坐标.y;
	in.z = 中心坐标.z;
	in.w = 1.0;

	auto out = 视图矩阵 * in;
	//transform_point(out, modelMatrix, in);  //乘以模型视图矩阵
	in = 投影矩阵 * out;
	//transform_point(in, projMatrix, out);   //乘以投影矩阵

	//齐次向量的第四项不能为0
	if (in.w != 0.0) {
		in.x /= in.w;
		in.y /= in.w;
		in.z /= in.w;
	}

	//视口向量的作用
	return { rect.offset.x + (1 + in.x) * rect.extent.x * 0.5f, rect.offset.y + (1 + in.y) * rect.extent.y * 0.5f };

}

/*bool f_graph_AABB射线相交(const vec3& start, const vec3& dir, const vec3& min, const vec3& max, float32& t) {
	float32 l1 = (min.x - start.x) * dir.x;
	float32 l2 = (max.x - start.x) * dir.x;

	float32 lmin = DEF_Max(l1, l2);
	float32	lmax = DEF_Min(l1, l2);

	l1 = (min.y - start.y) * dir.y;
	l2 = (max.y - start.y) * dir.y;
	lmin = DEF_Max(DEF_Min(l1, l2), lmin);
	lmax = DEF_Min(DEF_Max(l1, l2), lmax);

	l1 = (min.z - start.z) * dir.z;
	l2 = (max.z - start.z) * dir.z;
	lmin = DEF_Max(DEF_Min(l1, l2), lmin);
	lmax = DEF_Min(DEF_Max(l1, l2), lmax);


	bool hit = ((lmax >= 0.0f) & (lmax >= lmin));
	if (hit) t = lmin;

	return hit;
}*/
















#include "A_CUDA数学Core/aabbtree.h"
#include "A_CUDA数学Core/mesh.h"

void f_Voxelize遮罩(const Vec3* 顶点, int 顶点num, const uint32* 索引, int 索引数量, uint32 width, uint32 height, uint32 depth, uint8* volume, Vec3 minExtents, Vec3 maxExtents) {
	memset(volume, 0, sizeof(uint8) * width * height * depth);

	// build an aabb tree of the mesh
	AABBTree tree(顶点, 顶点num, (const uint32*)索引, 索引数量 / 3);

	// parity count method, single pass
	const Vec3 extents(maxExtents - minExtents);
	const Vec3 delta(extents.x / width, extents.y / height, extents.z / depth);
	const Vec3 offset(0.5f * delta.x, 0.5f * delta.y, 0.5f * delta.z);

	// this is the bias we apply to step 'off' a triangle we hit, not very robust
	const float eps = 0.00001f * extents.z;

	for (uint32 x = 0; x < width; ++x)
	{
		for (uint32 y = 0; y < height; ++y)
		{
			bool inside = false;

			Vec3 rayDir = Vec3(0.0f, 0.0f, 1.0f);
			Vec3 rayStart = minExtents + Vec3(x * delta.x + offset.x, y * delta.y + offset.y, 0.0f);

			uint32 lastTri = uint32(-1);
			for (;;)
			{
				// calculate ray start
				float t, u, v, w, s;
				uint32 tri;

				if (tree.TraceRay(rayStart, rayDir, t, u, v, w, s, tri))
				{
					// calculate cell in which intersection occurred
					const float zpos = rayStart.z + t * rayDir.z;
					const float zhit = (zpos - minExtents.z) / delta.z;

					uint32_t z = uint32_t(floorf((rayStart.z - minExtents.z) / delta.z + 0.5f));
					uint32_t zend = std::min(uint32_t(floorf(zhit + 0.5f)), depth - 1);

					if (inside)
					{
						// march along column setting bits 
						for (uint32_t k = z; k < zend; ++k)
							volume[k * width * height + y * width + x] = 0xff;
					}

					inside = !inside;

					// we hit the tri we started from
					if (tri == lastTri)
						printf("Error self-intersect\n");
					lastTri = tri;

					rayStart += rayDir * (t + eps);

				}
				else
					break;
			}
		}
	}
}

bool f_graph_点是否在矩形内(const S_Rect2Df& rect, vec2* v, uint32 num) {

	uint32 拆分次数 = num / 8;
	uint32 多出次数 = num - 拆分次数 * 8;


	auto 线点Ax = _mm256_set1_ps(rect.offset.x);
	auto 线点Ay = _mm256_set1_ps(rect.offset.y);

	auto 线点Bx = _mm256_set1_ps(rect.extent.x);
	auto 线点By = _mm256_set1_ps(rect.extent.y);

	__m256 左边X;
	__m256 左边Y;
	for (uint32 i = 0; i < 拆分次数; ++i) {

	}
	for (uint32 i = 0; i < num; ++i) {
		//lines[i] = points[i].row0;
		左边X.m256_f32[i] = v[i].x;
		左边Y.m256_f32[i] = v[i].y;
		//_mm256_mask_load_ps(左边, 8, )

	}


	auto A是否在区域内 = _mm256_and_ps(_mm256_cmp_ps(左边X, 线点Ax, _CMP_GE_OS), _mm256_cmp_ps(左边Y, 线点Ay, _CMP_GE_OS));
	auto B是否在区域内 = _mm256_and_ps(_mm256_cmp_ps(左边X, 线点By, _CMP_LE_OQ), _mm256_cmp_ps(左边Y, 线点By, _CMP_LE_OQ));

	auto yes_no = _mm256_and_ps(A是否在区域内, B是否在区域内);

	return false;
}

int32 f_graph_线段相交(const S_Line& 主线, const vec2* lines, uint32 num) {
	vec2 ScissorsVec = (主线.start - 主线.end);
	//if (ScissorsVec <= 0.000001) return -1;
	//vec2 法线投影 = (vec2{ 主线.start.y - 主线.end.y, 主线.end.x - 主线.start.x });

	--num;
	for (int32 i = 0; i < num; ++i) {
		uint32 endID = i + 1;

		//auto AB = vec_cross(lines[i] - 主线.start, 主线.start - lines[endID]);
		//auto AC = vec_cross(lines[i] - 主线.end, 主线.end - lines[endID]);
		//
		//auto DB = vec_cross(主线.start - lines[i], 主线.end - lines[i]);
		//auto DC = vec_cross(主线.start - lines[endID], 主线.end - lines[endID]);
		//
		//float32 d1 = vec_dot(AB, AC);
		//float32 d2 = vec_dot(DB, DC);
		//if (d1 * d2 > 0) {
		//	return i;
		//}


		//float32 距离c = 主线.end.x * 法线投影.x + 主线.end.y * 法线投影.y;
		//float32 距离a = lines[i].x * 法线投影.x + lines[i].y * 法线投影.y;
		//float32 距离b = lines[endID].x * 法线投影.x + lines[endID].y * 法线投影.y;
		//if((距离a - 距离c) * (距离b - 距离c) < 0) return i; 


		//auto AC = (lines[endID] - lines[i]);
		//auto A1 = (lines[i] - 主线.start);
		//auto A2 = (lines[endID] - 主线.end);
		//还是自个的方法靠谱'_'
		auto AC = vec_normalize(lines[endID] - lines[i]);
		auto A1 = vec_normalize(lines[i] - 主线.start);
		auto A2 = vec_normalize(lines[endID] - 主线.end);
		float32 d1 = vec_dot((vec_cross(ScissorsVec, A1)), (vec_cross(ScissorsVec, A2)));
		float32 d2 = vec_dot((vec_cross(AC, A1)), (vec_cross(AC, A2)));
		if (d1 <= 0 && d2 <= 0) {
			return i;
		}
	}
	return -1;
}


Inline bool f_graph_射线与AABB相交(const vec3& s, const vec3& dir, const S_Bounding& box, float32* t = nullptr) {
	float32
		l1 = (box.min.x - s.x) * dir.x,
		l2 = (box.max.x - s.x) * dir.x,
		lmin = DEF_Min(l1, l2),
		lmax = DEF_Max(l1, l2);

	l1 = (box.min.y - s.y) * dir.y;
	l2 = (box.max.y - s.y) * dir.y;
	lmin = DEF_Max(DEF_Min(l1, l2), lmin);
	lmax = DEF_Min(DEF_Max(l1, l2), lmax);

	l1 = (box.min.z - s.z) * dir.z;
	l2 = (box.max.z - s.z) * dir.z;
	lmin = DEF_Max(DEF_Min(l1, l2), lmin);
	lmax = DEF_Min(DEF_Max(l1, l2), lmax);

	bool hit = ((lmax >= 0.f) & (lmax >= lmin));
	if (hit && t)
		*t = lmin;
	return hit;
}

bool f_graph_射线与变换AABB相交(const S_Bounding& box, const vec3& s, const vec3& dir, vec3* 相交坐标, float32* 相交距离) {
	bool 射线平行于边界面 = true;

	vec3 txyz = {-1,-1,-1};
	if (dir.x != 0) txyz.x = (box.min.x - s.x) / dir.x;
	if (dir.y != 0) txyz.y = (box.min.y - s.y) / dir.y;
	if (dir.z != 0) txyz.z = (box.min.z - s.z) / dir.z;

	if (s.x != box.min.x) 射线平行于边界面 = false;
	if (s.y != box.min.y) 射线平行于边界面 = false;
	if (s.z != box.min.z) 射线平行于边界面 = false;

	if(射线平行于边界面) {
		(*相交距离) = 0;
		return false;
	}
	
	float32 maxt = txyz.x;
	int32 相交轴 = 0;
	if (txyz.y > maxt) {
		maxt = txyz.y;
		相交轴 = 1;
	}

	if (txyz.z > maxt) {
		maxt = txyz.z;
		相交轴 = 2;
	}
	if(maxt < 0) return false;

	*相交坐标 = s + dir * maxt;

	if ((相交坐标->x < box.min.x || 相交坐标->x > box.max.x) && 相交轴 != 0) return false;
	if ((相交坐标->y < box.min.y || 相交坐标->x > box.max.y) && 相交轴 != 1) return false;
	if ((相交坐标->z < box.min.z || 相交坐标->z > box.max.z) && 相交轴 != 2) return false;

	(*相交距离) = maxt;
	return true;
}







